#include "Batch.h"

#include <iostream>
//#include "HelperFunctions.h"

void Batch::setBatchLimits( PO & po, int begin_frame_counter, int & batch_begin_frame, int & batch_num_of_frames )
{
	if ( begin_frame_counter + po.batch_size > po.begin_frame + po.num_of_frames ) 
	{
		batch_num_of_frames = po.begin_frame + po.num_of_frames - begin_frame_counter;
	} 
	else 
	{
		batch_num_of_frames = po.batch_size;
	}
	
	batch_begin_frame = begin_frame_counter;
	if ( !po.first_graph )
	{
		batch_begin_frame--;
		batch_num_of_frames++;
	}

	batch_end_frame = batch_begin_frame + batch_num_of_frames;
}

void Batch::getSource()
{
	if ( sources.size() == 0 ) 
	{
		source = p_po->vsource;
	} 
	else 
	{
		location l = sources[0];
		source = (l.y-1) * p_po->pom_x +l.x;
		sources.erase(sources.begin());
	}
}

void Batch::readFile( int frame, vector<double> & proba )
{
	string frame_s = lexical_cast<string>(frame);
# ifndef USE_OLD_DAT_NOTATION
	string file_name = p_po->proba_path + frame_s + "-proba.dat";
# else
	string file_name = p_po->proba_path + "proba-f" + frame_s + ".dat";
#endif
	ifstream file(file_name.c_str());

	if(!file.is_open())
	{
		throw vbl::runtime_error( "[readFile] Proba file not found: " + file_name );
	} 
	else 
	{
		string line;
		while(getline(file, line))
		{
			// find 'space'-character, value behind it is probability, value before it is location
			for( unsigned int j = 0 ; j < line.size() ; j++ )
			{
				char c = line.at(j);
				if(c == ' ')
				{
					line = line.substr(j+1,line.size());
					break;
				}
			}
			double probability = lexical_cast<double>(line);

			proba.push_back(probability);

			// remove extremes, otherwise these numbers will create Inf numbers further on
			if( probability == 1 ) 
			{
				probability = 0.9999;
			}
			if( probability == 0 ) 
			{
				probability = 0.0001;
			}

			double cost = -(log(probability / (1-probability)) + offset);

			vertices.push_back(cost);
		}
		file.close();
	}
}

void Batch::readFiles()
{
	cout << "...Reading files..." << endl;
	
	boost::progress_display progress( batch_num_of_frames );
	for( int frame = batch_begin_frame ; frame < batch_end_frame ; frame++)
	{
		vector<double> proba;
		try
		{
			readFile(frame, proba);
		} catch( vbl::runtime_error e ) 
		{
			cout << e.what() << endl;
			exit(-1);
		}
		p_po->probabilities.push_back(proba);
		++progress;
	}
}

void Batch::getImages( int frame, vector<img::ImageRGB> & ims_out )
{
	vector<img::ImageRGB> ims;
	for( int cam = 0; cam < p_po->num_of_cams ; ++cam )
	{
		img::ImageG imRaw( p_po->image_width, p_po->image_height );
#ifndef USE_OLD_FILESYSTEM
		img::io::readRAW( p_po->files_per_camera[cam][ frame ], imRaw, imRaw.width(), imRaw.height() ); 
#else
		img::io::readRAW( p_po->image_path + "/" + p_po->files_per_camera[cam][ batch_begin_frame + frame ], imRaw, imRaw.width(), imRaw.height() ); 
#endif
		img::ImageRGB im;
		img::proc::demosaicPixelGrouping( imRaw, im );
		p_po->undistortImage( im, p_po->dist_map.first[cam], p_po->dist_map.second[cam] );
		img::rotateSafe< img::InterpolatorBilinear >( im, p_po->image_width/2, p_po->image_height/2, p_po->rot[cam] );
		ims.push_back(im);
	}
	ims_out = ims;
}

void Batch::getNeighbours( int location, vector<int> & neighbours )
{
	// the location in a timestep
	int location_in_timestep = location % p_po->pom_size;

	bool top = true;
	bool bottom = true;
	bool left = true;
	bool right = true;

	// check if the location is on an edge of the room
	if( location_in_timestep % p_po->pom_x == 0 ) 
	{ 
		// node is on left side of grid
		left = false;
	}
	if( location_in_timestep % p_po->pom_x == (p_po->pom_x - 1) ) 
	{
		// node is on right side of grid
		right = false;
	}
	if( location_in_timestep < p_po->pom_x ) 
	{
		// node is on top side of grid
		top = false;
	}
	if( location_in_timestep >= ( p_po->pom_size - p_po->pom_x ) ) 
	{
		// node is on bottom side of grid
		bottom = false;
	}

	if( location > vsink - p_po->pom_size - 2 ) 
	{
		if( !left || !right || !top || !bottom ) 
		{
			neighbours.push_back( p_po->vsource );
		}
		// TODO: vsink should be total_locations
		// location is situated the last timestep,
		// so it only needs to be connected to vsink
		// "-2" because of the extra locations vsource and vsink
		neighbours.push_back( vsink );
		return;
	}

	// correct location, vsource = 0 so all locations move up by 1
	location = location + 1;
	neighbours.push_back( location + p_po->pom_size);								// same location
	if( left ) 
	{
		neighbours.push_back( location - 1 + p_po->pom_size);						// left
	}
	if( right ) 
	{
		neighbours.push_back(location + 1 + p_po->pom_size);						// right
	}
	if( top ) 
	{
		neighbours.push_back(location - p_po->pom_x + p_po->pom_size);			// top
		if( left ) 
		{
			neighbours.push_back(location - p_po->pom_x - 1 + p_po->pom_size);	// top left
		}
		if( right ) 
		{
			neighbours.push_back(location - p_po->pom_x + 1 + p_po->pom_size);	// top right
		}
	}
	if( bottom ) 
	{
		neighbours.push_back(location + p_po->pom_x + p_po->pom_size);			// bottom
		if( left ) 
		{
			neighbours.push_back(location + p_po->pom_x - 1 + p_po->pom_size);	// bottom left
		}
		if( right ) 
		{
			neighbours.push_back(location + p_po->pom_x + 1 + p_po->pom_size);	// bottom right
		}
	}
	if( !left || !right || !top || !bottom ) 
	{
			neighbours.push_back( p_po->vsource );
			neighbours.push_back( vsink );
	}
}

double Batch::getProbability( int frame, int location, int range )
{
	double probability = p_po->probabilities[frame][location]; // TODO: is frame correct over multiple batches?
	int count = 1;
	for( int i = 1 ; i <= range ; i++ )
	{
		if ( frame+i >= 0 && frame+i < p_po->probabilities.size() )
		{
			probability += p_po->probabilities[frame+i][location];
			count++;
		}
	}
	for( int i = 1 ; i <= range ; i++ )
	{
		if ( frame-i >= 0 && frame-i < p_po->probabilities.size() )
		{
			probability += p_po->probabilities[frame-i][location];
			count++;
		}
	}
	return probability/count;
}

void Batch::getAppearance( int frame, rectangle r, img::ImageRGB & im, img::ImageRGB & background, AppearanceHist & apHist )
{
	img::Triple<> maskColor(0,204,153);

	img::ImageRGB cutout( r.xmax - r.xmin, r.ymax - r.ymin );
	img::ImageRGB backgroundCutOut( r.xmax - r.xmin , r.ymax - r.ymin );

	img::copy( im, r.xmin, r.ymin, r.xmax - r.xmin, r.ymax - r.ymin, cutout);
	img::copy( background , r.xmin , r.ymin , r.xmax - r.xmin , r.ymax - r.ymin , backgroundCutOut );

	apHist.add( cutout, backgroundCutOut, maskColor );
	apHist.normalize();
}

bool Batch::isOccluded( int location, int cam )
{
	int location_in_time_step = location % p_po->pom_size;
	map<int,vector<int>>::iterator it = p_po->occlusion_map[ cam ].find( location_in_time_step );
	if( it == p_po->occlusion_map[ cam ].end() )
	{
		return false;
	}
	else
	{
		bool toReturn = false;
		vector<int> occ = it->second;
		int frame = (int) floor( ((double) location ) / p_po->pom_size );
		for( int i = 0 ; i < occ.size() ; i++ )
		{
			double val = p_po->probabilities[ batch_begin_frame - p_po->begin_frame + frame ][ location_in_time_step-1 ];
			if( val > 0.5 )
			{
				toReturn = true;
				break;
			}
		}
		return toReturn;
	}
}

void Batch::getAppearance2( int frame, int location, vector<img::ImageRGB> & ims, vector<AppearanceHist> & appearance,
						  vector<bool> & visibility )
{
	pair<int,int> ap_loc( frame, location );
	hist_map_t::iterator it = hist_map.find(ap_loc);
	if( it  != hist_map.end() )
	{
		appearance = it->second;
		visibility = vis_map[ ap_loc ];
	}
	else 
	{
		for( int cam = 0 ; cam < p_po->num_of_cams ; cam++ )
		{
			rectangle r = p_po->pom_locations[cam][location-1];
			if( !isOccluded( location, cam ) && r.isVisible )
			{
				img::ImageRGB background;
#ifndef USE_CAS_DATASET
				string backgroundImPath = p_po->background_path + lexical_cast<string>(frame) + "_" + lexical_cast<string>(cam) + 
					"-result.png";
#else
				string backgroundImPath = p_po->background_path + "result-f" + lexical_cast<string>(frame) + "-c" + 
					lexical_cast<string>(cam) + ".png";
#endif
				try
				{
					img::io::read(backgroundImPath , background );
				} 
				catch( vbl::runtime_error e ) 
				{
					cout << " Could not read background file: " << e.what() << endl;
					exit(1);
				}
				AppearanceHist apHist;
				getAppearance( frame, r, ims[cam], background, apHist );
				appearance.push_back(apHist);
				visibility.push_back( true );
			}
			else
			{
				AppearanceHist apHist;
				appearance.push_back(apHist);
				visibility.push_back( false );
			}
		}
		hist_map[ap_loc] = appearance;
		vis_map[ ap_loc ] = visibility;
	}
}

void Batch::calculateCostsAppearance( double cost, int frame, int location, vector<int> & neighbours, vector<img::ImageRGB> & ims1, 
							  vector<img::ImageRGB> & ims2, vector<double> & costs )
{
	// get appearance
	vector<AppearanceHist> appearance;
	//getAppearance( frame, location, ims1, appearance );
	
	// for every neighbour get appearance
	vector<vector<AppearanceHist>> neighbourAppearances;
	for( unsigned int n = 0 ; n < neighbours.size() ; n++ )
	{
		if( neighbours[n] != p_po->vsource && neighbours[n] != vsink )
		{
			int neighbour = neighbours[n] % p_po->pom_size;
			vector<AppearanceHist> neighbourAp;
			//getAppearance( frame+1, neighbour, ims2, neighbourAp );
			neighbourAppearances.push_back(neighbourAp);
		}
	}

	// compare using bhatta
	vector<double> distances;
	computeBhattacharyyaCoefficients( appearance, neighbourAppearances, distances );

	// reweight costs
	reweighCosts( cost, distances, costs );
}

void Batch::reweighCosts( double cost, vector<double> & distances, vector<double> & costs )
{
	double compareCost = distances[0];
	for( unsigned int d = 0 ; d < distances.size() ; d++ )
	{
		if( distances[d] == -1 )
		{
			costs.push_back( cost );
		} 
		else 
		{
			double scalar;
			if( cost <= 0 )
			{
				scalar = pow(((distances[d]-compareCost)/compareCost)+1,10);
			} 
			else 
			{
				scalar = pow(1-((distances[d]-compareCost)/compareCost),10);
			}
			costs.push_back( scalar * cost );
		}
	}
}

void Batch::computeBhattacharyyaCoefficients( vector<AppearanceHist> & appearance, 
											 vector<vector<AppearanceHist>> & neighbourAppearances, 
											 vector<double> & distances )
{
	AppearanceHist combinedHist;
	// Combine histograms from different cameras for all parts and keep them normalized
	for( unsigned int n = 0; n < appearance.size() ; ++n )
	{
		combinedHist.add( appearance[n], 1.0 / (double)(n + 1) );
	}

	for( unsigned int n = 0 ; n < neighbourAppearances.size() ; ++n )
	{
		vector<AppearanceHist> neighbourAppearance = neighbourAppearances[n];
		if( neighbourAppearance.size() != appearance.size() )
		{
			// not same ammount of visible cams
			distances.push_back(-1);
		}
		else
		{
			AppearanceHist combinedHistN;
			for( unsigned int na = 0; na < neighbourAppearance.size() ; ++na )
			{
				combinedHistN.add( neighbourAppearance[na], 1.0 / (double)(na + 1) );
			}
			distances.push_back( combinedHist.bhattacharyyaCoefficient( combinedHistN ));
		}
	}
}

void Batch::createGraph( graph_t & g )
{
	cout << "...Creating Graph..." << endl;

	if ( p_po->first_graph ) 
	{
		// connect vsource to all nodes in first timestep
		for( unsigned int i = 1 ; i <= p_po->pom_size ; i++ )
		{
			add_edge( p_po->vsource , i , 0 , g );
		}
	} 
	else 
	{
		for( unsigned int i = 0 ; i < sources.size() ; i++ ) 
		{
			location loc = sources[i];
			int l = (loc.y-1)*p_po->pom_x + loc.x;
			add_edge( p_po->vsource , l , 0 , g );
		}
	}
	
	vector<img::ImageRGB> ims1, ims2;
	if( p_po->use_appearance )
	{
		getImages( 0, ims1 );
		getImages( 1, ims2 );
	}

	bool frame_change = false;
	int current_frame = 0;

	boost::progress_display progress( vertices.size() );
	for( unsigned int i = 0 ; i < vertices.size() ; i++ )
	{
		++progress;
		int frame = (int) floor( ((double) i ) / p_po->pom_size );

		if( frame != current_frame )
		{
			current_frame = frame;
			frame_change = true;
		}

		if( p_po->use_appearance && frame_change && frame != batch_num_of_frames-1 )
		{
			ims1 = ims2;
			getImages( frame+1, ims2 );
			frame_change = false;
		}
		double cost = vertices[i];
		vector<int> neighbours;
		getNeighbours( i, neighbours );

		vector<double> costs;
		
		int location = ( i % p_po->pom_size );
		double probability = getProbability( frame, location, 0 );
		// TODO make threshold a variable
		if( p_po->use_appearance && probability > 0.05 && frame != batch_num_of_frames-1 )
		{
			calculateCostsAppearance( cost, frame, location, neighbours, ims1, ims2, costs );
		}
		else
		{
			for( unsigned int neighbour = 0 ; neighbour < neighbours.size() ; neighbour++ )
			{
				costs.push_back(cost);
			}
		}

		int trueNeighbourCounter = 0; // i.e. not vsource and vsink
		for( unsigned int j = 0 ; j < neighbours.size() ; j++ ) 
		{
			// TODO: ugly coding, +1 is to correct for nodes beginning at 0 (vsource == 0)
			if( neighbours[j] == p_po->vsource ) 
			{
				if ( i+1 > p_po->pom_size ) 
				{
					add_edge( p_po->vsource , i+1 , 0 , g );
				}
			} 
			else if( neighbours[j] == vsink )
			{
				add_edge( i+1 , vsink , cost , g );
			}
			else 
			{
				add_edge( i+1 , neighbours[j] , costs[trueNeighbourCounter] , g );
				trueNeighbourCounter++;
			}
		}
	}
}

void Batch::find_shortest_path_dag( graph_t & g, vector<int> & path ) 
{
	// find graph properties
	distance_map = get( vertex_distance , g );
	weight_map_t weight_map = get( edge_weight, g );
	vector<default_color_type> color(num_vertices(g));
	vector<size_t> predecessor_map(num_vertices(g));
	
	// initialize variables for dag_shortest_paths
	default_dijkstra_visitor vis;
	closed_plus<double> combine;
	less<double> compare;
	
	// perform dag_shortest_paths
	dag_shortest_paths( g , source , distance_map, weight_map, &color[0], &predecessor_map[0], vis, compare, combine, 
		(numeric_limits<int>::max)(), 0);

	// find shortest path by tracing back from vsink
	reconstructPath( predecessor_map , source , vsink , path );
}

void Batch::reconstructPath( vector<size_t> & predecessorMap , int source , int vsink , vector<int> & path ) 
{
	int v = vsink;
	while( v != source ) 
	{
		path.insert( path.begin(), v );
		int v_temp = predecessorMap[v];
		if( v == v_temp ) 
		{
			cout << "Node is its own predecessor" << endl;
			exit(0);
		} 
		else 
		{
			v = v_temp;
		}
	}
	path.insert( path.begin(), source );
	if ( source != p_po->vsource ) {
		path.insert( path.begin(), p_po->vsource );
	}
}

void Batch::find_shortest_path_dijkstra( graph_t & g, vector<int> & path ) 
{
	vector<size_t> predecessorMap(num_vertices(g));
       
	distance_map = get( vertex_distance , g );
	weight_map_t weight_map = get( edge_weight , g );
	index_map_t index_map = get( vertex_index , g );
	
	// initialize variables for dijkstra_shortest_paths
	default_dijkstra_visitor vis;
	closed_plus<double> combine;
	less<double> compare;
    
	dijkstra_shortest_paths(g, source, &predecessorMap[0], distance_map, weight_map, index_map, compare, combine, 
		(numeric_limits<double>::max)(), 0, vis);

	// find shortest path by tracing back from vsink
	reconstructPath( predecessorMap , source , vsink , path );
}

void Batch::getShortestPath( graph_t & g, vector<int> & path )
{
	if( paths.size() == 0 )
	{
		find_shortest_path_dag( g, path );
	} 
	else
	{
		find_shortest_path_dijkstra( g, path );
	}
}

double Batch::pathsCost() 
{
	weight_map_t weight_map = get( edge_weight, g_original );
	double w = 0.0;

	for( unsigned int i = 0 ; i < paths.size() ; i++ )
	{
		vector<int> path = paths[i];
		w += pathCost( path, weight_map );
	}

	return w;
}

double Batch::pathCost( vector<int> & path, weight_map_t & weight_map ) 
{
	double w = 0.0;

	for( unsigned int j = 0 ; j < path.size() ; j++ ) 
	{
		if( j != 0 ) 
		{
			edge_t e;
			bool found;
			tie(e, found) = edge(path[j-1],path[j], g_original );
			if( found )
			{
				w += get(weight_map, e);
			}
			else
			{
				cerr << "Unable to find edge to calculate costs." << endl;
				exit(-1);
			}
		}
	}

	return w;
}

void Batch::copy_weight_and_distance_map( graph_t & g , map<pair<int, int>,double> & weights , distance_map_t & distance_map, 
								  vector_property_map<double> & distances ) 
{
	weight_map_t weight_map = get( edge_weight, g );
	pair<vertex_iter, vertex_iter> vi;
	for ( vi = boost::vertices( g ) ; vi.first != vi.second ; vi.first++ ) 
	{
		distances[*vi.first] = distance_map[*vi.first];
		graph_traits<graph_t>::out_edge_iterator out_it, out_it_end;
		tie( out_it, out_it_end ) = out_edges( *vi.first , g );
		for ( ; out_it != out_it_end ; out_it++ ) 
		{
			vertex_t t = target( *out_it , g );
			double w = get(weight_map, *out_it);
			pair<int, int> key(*vi.first,t);
			weights.insert(make_pair(key, w));
		}
	}	
}

void Batch::extendGraph( graph_t & g , map<int,int> & vertexPairs) 
{
	for ( unsigned int i = 0 ; i < paths.size() ; i++ ) 
	{
		extendGraphPath( g , paths[i] , vertexPairs );
	}
}

void Batch::extendGraphPath( graph_t & g , vector<int> & path , map<int,int> & vertexPairs ) 
{
	weight_map_t weight_map = get( edge_weight , g );
	vector<int> new_path;

	for ( unsigned int i = 0 ; i < path.size() ; i++ ) 
	{
		new_path.push_back( path[i] );

		if ( path[i] != p_po->vsource && path[i] != vsink ) 
		{
			// add new vertex
			vertex_t v = add_vertex( g );
			new_path.push_back( v );
			vertexPairs.insert(make_pair(v,path[i]));
			// connect all out edges of path[i] to new vertex
			graph_traits<graph_t>::out_edge_iterator out_it, out_it_end;
			tie( out_it, out_it_end ) = out_edges( path[i] , g );
			vector<vertex_t> toRemove;
			double cost = get( weight_map , *out_it );
			for ( ; out_it != out_it_end ; out_it++ ) 
			{
				vertex_t t = target( *out_it , g );
				add_edge( v , t , cost, g );
				toRemove.push_back( t );
			}
			
			// remove all out edges from path[i]
			for ( unsigned int j = 0 ; j < toRemove.size() ; j++ ) 
			{
				remove_edge( path[i] , toRemove[j] , g );
			}
			
			// connect path[i] to new vertex with cost 0
			add_edge( path[i] , v , 0 , g );
		}
	}

	// for all vertices in the path, reverse direction and negate cost
	for ( unsigned int i = 1 ; i < new_path.size() ; i++ ) 
	{
		edge_t e;
		bool found;
		tie( e , found ) = edge( new_path[i-1] , new_path[i], g );
		if( found ) 
		{
			double cost = get( weight_map , e );
			remove_edge( new_path[i-1] , new_path[i], g );
			add_edge( new_path[i] , new_path[i-1] , -cost , g );
		} 
		else 
		{
			cout << "Edge not found while reversing cost and direction of a path" << endl;
			cout << "Edge going from " << new_path[i-1] << " to " << new_path[i] << endl;
			exit(1);
		}
	}
}

void Batch::transformEdgeCost( graph_t & g , map<pair<int,int>, double> & weights, vector_property_map<double> & distance_map, 
						 map<int,int> & vertexPairs ) 
{
	edge_iter ei, ei_end;
	tie( ei , ei_end ) = edges( g );
	for( ; ei != ei_end ; ei++ ) 
	{
		vertex_t s = boost::source( *ei , g );
		vertex_t t = boost::target( *ei , g );

		pair<int,int> key(s,t);
		map<pair<int,int>, double>::iterator wi = weights.find(key);
		double cost;
		if( wi != weights.end() ) 
		{
			cost = (*wi).second;
		} 
		else 
		{
			if( t == 0 ) 
			{
				pair<int,int> key(t,s);
				cost = -weights[key];
			} 
			else if( t >= p_po->total_locations ) 
			{
				vertex_t pairedVertex = vertexPairs[t];
				pair<int,int> key(pairedVertex,s);
				cost = -weights[key];
			} 
			else if( s >= p_po->total_locations ) 
			{
				vertex_t pairedVertex = vertexPairs[s];
				if ( pairedVertex == t ) 
				{
					cost = 0;
				} 
				else 
				{
					pair<int,int> key(pairedVertex,t);
					cost = weights[key];
				}
			} 
			else 
			{
				cost = get( edge_weight , g , *ei );
			}
		}

		double distanceSource;
		double distanceTarget;

		distanceSource = distance_map[s];
 
		if ( t >= p_po->total_locations ) 
		{
			distanceTarget = distanceSource + cost;
			distance_map[t] = distanceTarget;
		} 
		else 
		{
			distanceTarget = distance_map[t];
		}
		
		if ( distanceTarget > distanceSource + cost ) 
		{
			distanceTarget = distanceSource + cost;
			distance_map[t] = distanceTarget;
		}
		
		double new_cost = cost + distanceSource - distanceTarget;
		if( new_cost < 0 && new_cost > -0.001 ) 
		{
			new_cost = 0;
		}
		put( edge_weight , g , *ei , new_cost );
	}
}

void Batch::find_and_remove_competing_path( int vertex , unsigned int & continueLocation, vector<int> & toReturn )
{
	int path_location;
	for( unsigned int i = 0 ; i < paths.size() ; i++ ) 
	{
		vector<int> path = paths[i];
		for( unsigned int j = 0 ; j < path.size() ; j++ ) 
		{
			int toCompare = path[j];
			if( vertex == toCompare ) 
			{
				toReturn = paths[i];
				path_location = i;
				continueLocation = j;
				break;
			}
		}
	}
	paths.erase(paths.begin() + path_location);
}

int Batch::locationOf( int toSearch, vector<int> path ) 
{
	for( unsigned int i = 0 ; i < path.size() ; i++ ) 
	{
		if( path[i] == toSearch ) 
		{
			return i;
		}
	}
	return -1;
}

void Batch::find_leave_point( vector<int> & path, vector<int> & competing_path, int hit_location, unsigned int & leave_location, 
								unsigned int & split_location )
{
	split_location = locationOf( path[hit_location] , competing_path );
	while( true ) 
	{
		split_location -= 1;
		leave_location = locationOf( competing_path[split_location] , path );
		if ( leave_location == -1 ) 
		{
			leave_location = locationOf( competing_path[split_location+1] , path ) + 2;
			break;
		}
	}
}

void Batch::interlace_and_augment( vector<int> & path ) 
{
	// applies to path
	unsigned int hit_location = 0;
	unsigned int leave_location = 0;
	// applies to crossing path
	unsigned int split_location = 0;
	unsigned int continueLocation = 0;
	vector<int> competing_path;
	bool interlace_found = false;
	for( unsigned int i = 0 ; i < path.size() ; i++ ) 
	{
		if( i != 0 ) 
		{
			edge_t e;
			bool found;
			tie(e, found) = edge(path[i-1],path[i], g_original );
			if( !found ) 
			{
				// not in orignal graph, so must be one of the edges going backwards
				hit_location = i-1;
				find_and_remove_competing_path( path[hit_location] , continueLocation, competing_path );
				interlace_found	= true;
				find_leave_point( path , competing_path , hit_location , leave_location , split_location );
				break;
			}
		}
	}
	if( interlace_found ) 
	{
		vector<int> new_path_1;
		vector<int> new_path_2;
		for( unsigned int i = 0 ; i < hit_location ; i++ ) 
		{
			new_path_1.push_back(path[i]);
		}
		for( unsigned int i = continueLocation ; i < competing_path.size() ; i++ )
		{
			new_path_1.push_back(competing_path[i]);
		}
		for( unsigned int i = 0 ; i <= split_location ; i++ ) 
		{
			new_path_2.push_back(competing_path[i]);
		}
		for( unsigned int i = leave_location ; i < path.size() ; i++ )
		{
			new_path_2.push_back(path[i]);
		}
		//paths.push_back( new_path_1 );
		paths.push_back( new_path_1 );
		interlace_and_augment( new_path_2 );
	}
	else 
	{
		paths.push_back( path );
	}
}

void put( int key, int value, map<int,vector<int>> & map_ )
{
	vector<int> values;
	map<int,vector<int>>::iterator it = map_.find(key);
	if( it != map_.end() )
	{
		values = it->second;
	}
	values.push_back( value );
	map_[ key ] = values;
}

void put( pair<int,int> key, pair<int,int> value, map<pair<int,int>,vector<pair<int,int>>> & map_ )
{
	vector<pair<int,int>> values;
	map<pair<int,int>,vector<pair<int,int>>>::iterator it = map_.find(key);
	if( it != map_.end() )
	{
		values = it->second;
	}
	values.push_back( value );
	map_[ key ] = values;
}

int Batch::intToTimestep( int i )
{
	return (int) floor( ((double) i-1) / p_po->pom_size );
}

pair<int,int> Batch::intToXY( int i )
{
	i = ( ( i - 1 ) % p_po->pom_size ) + 1;
	int locx = ((i-1) % p_po->pom_x )+1;
	int locy = (( i - locx ) / p_po->pom_x )+1;
	return make_pair(locx, locy);
}

bool Batch::withinRange( int range, int loc, int compare, pair<int,int> & diff )
{
	int ts = intToTimestep(loc);
	int ts_comp = intToTimestep( compare );
	if( ts != ts_comp )
	{
		return false;
	}

	pair<int,int> locXY = intToXY( loc );
	pair<int,int> locXY_comp = intToXY( compare );

	int x_diff = locXY_comp.first - locXY.first;
	int y_diff = locXY_comp.second - locXY.second;

	if( abs(x_diff) + abs(y_diff) <= range )
	{
		diff = make_pair( x_diff, y_diff );
		return true;
	}
	else
	{
		return false;
	}
}

void addVertex( int vertex, vector<int> & vertices, map<int, bool> & availableVertices )
{
	map<int, bool>::iterator it = availableVertices.find(vertex);
	if( it != availableVertices.end() )
	{
		return;
	}
	else
	{
		vertices.push_back( vertex );
		availableVertices[vertex] = true;
	}
}

void Batch::getVerticesInRange( int vertex, pair<int,int> diff, vector<int> & extraVertices, 
								map<int, bool> & availableVertices )
{
	if( diff.first == 0 ) // difference is in y direction
	{
		if( diff.second < 0 )
		{
			for( int i = 0 ; i >= diff.second ; i-- )
			{
				addVertex( vertex + i*p_po->pom_x, extraVertices, availableVertices );
			}
		}
		else
		{
			for( int i = 0 ; i <= diff.second ; i++ )
			{
				addVertex( vertex + i*p_po->pom_x, extraVertices, availableVertices );
			}
		}
	}
	else if( diff.second == 0 ) // difference is in x direction
	{
		if( diff.first < 0 )
		{
			for( int i = 0 ; i >= diff.first ; i-- )
			{
				addVertex( vertex + i, extraVertices, availableVertices );
			}
		}
		else
		{
			for( int i = 0 ; i <= diff.first ; i++ )
			{
				addVertex( vertex + i, extraVertices, availableVertices );
			}
		}
	}
	else // both x and y directions are non-zero
	{
		if( diff.first < 0 && diff.second < 0 )
		{
			for( int i = 0 ; i >= diff.first ; i-- )
			{
				for( int j = 0 ; j >= diff.second ; j-- )
				{
					addVertex( vertex + i + j*p_po->pom_x, extraVertices, availableVertices );
				}
			}
		}
		else if( diff.first > 0 && diff.second < 0 )
		{
			for( int i = 0 ; i <= diff.first ; i++ )
			{
				for( int j = 0 ; j >= diff.second ; j-- )
				{
					addVertex( vertex + i + j*p_po->pom_x, extraVertices, availableVertices );
				}
			}
		}
		else if( diff.first < 0 && diff.second > 0 )
		{
			for( int i = 0 ; i >= diff.first ; i-- )
			{
				for( int j = 0 ; j <= diff.second ; j++ )
				{
					addVertex( vertex + i + j*p_po->pom_x, extraVertices, availableVertices );
				}
			}
		}
		else // both larger than 0
		{
			for( int i = 0 ; i <= diff.first ; i++ )
			{
				for( int j = 0 ; j <= diff.second ; j++ )
				{
					addVertex( vertex + i + j*p_po->pom_x, extraVertices, availableVertices );
				}
			}
		}
	}
}

void Batch::findVertices( vector<int> & LP_vertices, map<int, bool> & availableVertices )
{
	for( int i = 0 ; i < paths.size() ; i++ )
	{
		vector<int> path = paths[i];
		for( int j = 1 ; j < path.size()-1 ; j++ )
		{
			int vertex = path[j];
			addVertex( vertex, LP_vertices, availableVertices );
			for( int k = 0 ; k < paths.size() ; k++ )
			{
				if ( k != i )
				{
					vector<int> comparisonPath = paths[k];
					for( int l = 1 ; l < comparisonPath.size()-1 ; l++ )
					{
						pair<int,int> diff;
						if( withinRange( 3, vertex, comparisonPath[l], diff ) )
						{
							getVerticesInRange( vertex, diff, LP_vertices, availableVertices );
						}
					}
				}
			}
		}
	}
	/*for( int i = 0 ; i < paths.size() ; i++ )
	{
		vector<int> path = paths[i];
		for( int j = 0 ; j < path.size() ; j++ )
		{
			cout << path[j] << ", ";
		}
		cout << endl;
	}
	cout << endl;
	for( int i = 0 ; i < extraVertices.size() ; i++ )
	{
		cout << extraVertices[i] << ", ";
	}
	cout << endl;*/
}

bool isAvailable( int i, map<int, bool> m )
{
	map<int, bool>::iterator it = m.find(i);
	if( it != m.end() )
	{
		return true;
	}
	else
	{
		return false;
	}
}

void Batch::getObjectAppearance( int location, vector<AppearanceHist> & app, vector<vector<img::ImageRGB>> & ims,
								vector<bool> & cameraVisObj )
{
	int time_step = intToTimestep( location );
	int frame_number = batch_begin_frame + time_step;
	int location_in_timestep = location % p_po->pom_size;

	getAppearance2( frame_number, location_in_timestep, ims[time_step], app, cameraVisObj );
}

double KullbackLieblerDivergence( AppearanceHist t, AppearanceHist o )
{
	// TODO: check if these values are as expected
	double toReturn = 0;
	AppearanceHist::iterator t_it = t.begin();
	AppearanceHist::iterator t_it_end = t.end();
	AppearanceHist::iterator o_it = o.begin();
	for( t_it ; t_it != t_it_end ; t_it++, o_it++ )
	{
		if( *t_it > 0 && *o_it > 0 )
		{
			toReturn += ((*t_it) * log( (*t_it/ *o_it) ));
		}
	}
	return toReturn;
}

double Batch::getAppearanceValue( int group, int location, vector<vector<img::ImageRGB>> & ims )
{
	vector<AppearanceHist> templateApp = p_po->templates[group];
	vector<AppearanceHist> objectApp;
	vector<bool> visibility;
	getObjectAppearance( location, objectApp, ims, visibility );

	double sum = 0;
	int view_count = 0;
	for( int i = 0 ; i < templateApp.size() ; i++ )
	{
		if( p_po->camera_visibility[group][i] && visibility[i] )
		{
			sum += exp( -KullbackLieblerDivergence( templateApp[i], objectApp[i] ));
			view_count++;
		}
	}
	if( view_count == 0 )
	{
		return (1 / 3); // TODO: set to 1 / num_of_groups
	}
	else
	{
		return sum / view_count;
	}
}

void Batch::shitrit()
{
	cout << "Performing Shitrit appearance" << endl;
	int num_of_groups = paths.size(); // TODO: Set num_of_groups somewhere else
	int max_per_group = 1;

	vector<vector<img::ImageRGB>> ims;
	for( int i = 0 ; i < batch_num_of_frames ; i++ )
	{
		vector<img::ImageRGB> toAdd;
		getImages( i, toAdd );
		ims.push_back( toAdd );
	}

/*	
	vector<tuple<int,int,int>> edges; // done
	vector<pair<int,int>> sub_vertices;	
	vector<int> sub_vertices_no_group; 
	map<pair<int,int>,vector<pair<int,int>>> ins, outs; // done
	map<tuple<int,int,int>, int> edge_map; // done
	map<int, tuple<int,int,int>> edge_map_reverse; // done
	vector<vector<vector<tuple<int,int,int>>>> edge_per_timestep;
*/
	vector<int> LP_vertices;
	vector<pair<int,int>> LP_vertices_group;
	vector<tuple<int,int,int>> LP_edges;
	map<pair<int,int>,vector<pair<int,int>>> LP_in_edges, LP_out_edges;
	map<tuple<int,int,int>, int> LP_edge_map;
	map<int, tuple<int,int,int>> LP_edge_map_reverse;
	vector<vector<vector<tuple<int,int,int>>>> lp_edges_per_ts_per_group;
	map<int, bool> availability_map;

	for( int i = 0 ; i < batch_num_of_frames ; i++ )
	{
		vector<vector<tuple<int,int,int>>> toAdd;
		lp_edges_per_ts_per_group.push_back( toAdd );
		for( int j = 0 ; j < num_of_groups ; j++ )
		{
			vector<tuple<int,int,int>> toAdd2;
			lp_edges_per_ts_per_group[i].push_back( toAdd2 );
		}
	}

	int edge_counter = 1;

	findVertices( LP_vertices, availability_map );

	for( int i = 0 ; i < LP_vertices.size() ; i++ )
	{
		int v = LP_vertices[i];
		int time_step = intToTimestep( v );

		vector<int> neighbours;
		getNeighbours( v-1, neighbours );

		for( int group = 0 ; group < num_of_groups ; group++ )
		{	
			pair<int,int> gv( group, v );

			if( time_step == 0 )
			{
				tuple<int,int,int> edge( group, p_po->vsource, v );
				pair<int,int> gn( -1, p_po->vsource );
				LP_edges.push_back( edge );
				put( gn, gv, LP_out_edges);
				put( gv, gn, LP_in_edges);
				LP_edge_map[edge] = edge_counter;
				LP_edge_map_reverse[edge_counter] = edge;
				edge_counter++;
			} 
			else if( time_step == batch_num_of_frames-1 )
			{
				tuple<int,int,int> edge( group, v, vsink );
				pair<int,int> gn( -1, vsink );
				LP_edges.push_back( edge );
				put( gv, gn, LP_out_edges);
				put( gn, gv, LP_in_edges);
				LP_edge_map[edge] = edge_counter;
				LP_edge_map_reverse[edge_counter] = edge;
				lp_edges_per_ts_per_group[ time_step ][ group ].push_back(edge);
				edge_counter++;
			}

			// create same vertex per group
			LP_vertices_group.push_back( make_pair( group, v ));

			// create edge to every neighbour, per group
			for( int n = 0 ; n < neighbours.size() ; n++ )
			{
				if( isAvailable( neighbours[n], availability_map ))
				{
					tuple<int,int,int> edge( group, v, neighbours[n] );
					pair<int,int> gn( group, neighbours[n] );
					LP_edges.push_back( edge );
					put( gv, gn, LP_out_edges);
					put( gn, gv, LP_in_edges);
					LP_edge_map[edge] = edge_counter;
					LP_edge_map_reverse[edge_counter] = edge;
					lp_edges_per_ts_per_group[ time_step ][ group ].push_back(edge);
					edge_counter++;
				}
			}
		}
	}

/*
	for( int i = 0 ; i < batch_num_of_frames ; i++ )
	{
		vector<vector<tuple<int,int,int>>> toAdd;
		edge_per_timestep.push_back( toAdd );
		for( int j = 0 ; j < num_of_groups ; j++ )
		{
			vector<tuple<int,int,int>> toAdd2;
			edge_per_timestep[i].push_back( toAdd2 );
		}
	}

	edge_counter = 1;
	for( int i = 0 ; i < paths.size() ; i++ )
	{
		vector<int> path = paths[i];
		
		for( int j = 0 ; j < path.size()-1 ; j++ )
		{
			if( path[j] != p_po->vsource )
			{
				sub_vertices_no_group.push_back( path[j] );
			}
			for( int group = 0 ; group < num_of_groups ; group++ )
			{
				pair<int,int> vertex, out_vertex;
				tuple<int,int,int> edge( group, path[j], path[j+1] );
				if( path[j] == p_po->vsource )
				{
					vertex = make_pair( -1, path[j] );
				}
				else
				{
					vertex = make_pair( group, path[j] );
					int time_step = intToTimestep( path[j] );
					edge_per_timestep[time_step][group].push_back(edge);
				}
				if( path[j+1] == vsink )
				{
					out_vertex = make_pair( -1, path[j+1] );
				}
				else
				{
					out_vertex = make_pair( group, path[j+1] );
				}
				if( path[j] != p_po->vsource )
				{
					sub_vertices.push_back( vertex );
				}
				edge_map[edge] = edge_counter;
				edge_map_reverse[edge_counter] = edge;
				edges.push_back( edge );
				edge_counter++;
				put( vertex, out_vertex, outs);
				put( out_vertex, vertex, ins);
			}
		}
	}
*/
	lprec *lp;
 	int Ncol, *colno = NULL, ret = 0;
 	REAL *row = NULL;

	Ncol = edge_counter-1;

	lp = make_lp(0, Ncol);

	for( int i = 0 ; i < LP_edges.size() ; i++ )
	{
		int group = LP_edges[i].get<0>();
		int from = LP_edges[i].get<1>();
		int to = LP_edges[i].get<2>();
		string name = boost::lexical_cast<std::string>(group) + " : " + boost::lexical_cast<std::string>(from) + 
			" -> " + boost::lexical_cast<std::string>(to);
		char *a=new char[name.size()+1];
		a[name.size()]=0;
		memcpy(a,name.c_str(),name.size());
		set_col_name( lp, i+1, a );
	}

	// objective function
	int counter = 0;
	colno = (int *) malloc(Ncol * sizeof(*colno));
	row = (REAL *) malloc(Ncol * sizeof(*row));
	for( int i = 0 ; i < LP_edges.size() ; i++ )
	{
		//vector<pair<int,int>> out_vec = outs[sub_vertices[i]];
		//for( int j = 0 ; j < out_vec.size() ; j++ )
		//{
		//	tuple<int,int,int> edge( sub_vertices[i].first, sub_vertices[i].second, out_vec[j].second );
		tuple<int,int,int> edge = LP_edges[i];
		if( edge.get<1>() != p_po->vsource )
		{
			int edge_i = LP_edge_map[edge];
			colno[counter] = edge_i;
			double value = -vertices[edge.get<1>()-1];
			double appValue = getAppearanceValue( edge.get<0>(), edge.get<1>(), ims );
			row[counter++] = value*(appValue / num_of_groups);//sub_vertices[i].second-1]; // TODO put in appearance info here! This will not work, vertices still old one :)
		}
		//}
	}
	if(!set_obj_fnex(lp, counter, row, colno))
		ret = 1;
	//cout << ret << endl;

	free(colno);
	free(row);

	set_add_rowmode(lp, TRUE);

	// f <= 1
	for( int i = 0 ; i < LP_vertices.size() ; i++ )
	{
		counter = 0;
		colno = (int *) malloc(Ncol * sizeof(*colno));
		row = (REAL *) malloc(Ncol * sizeof(*row));
		for( int group = 0 ; group < num_of_groups ; group++ )
		{
			pair<int,int> vertex( group, LP_vertices[i] );
			vector<pair<int,int>> out_vec = LP_out_edges[vertex];
			for( int j = 0 ; j < out_vec.size() ; j++ )
			{
				tuple<int,int,int> edge( group, vertex.second, out_vec[j].second );
				int edge_i = LP_edge_map[edge];
				colno[counter] = edge_i;
				row[counter++] = 1;
			}
		}
		if(!add_constraintex( lp, counter, row, colno, LE, 1))
			ret = 2;
		free(colno);
		free(row);
	}
	//cout << ret << endl;

	// f >= 0
	colno = (int *) malloc(Ncol * sizeof(*colno));
    row = (REAL *) malloc(Ncol * sizeof(*row));
	for( int i = 1 ; i <= edge_counter-1 ; i++ )
	{
		colno[0] = i;
		row[0] = 1;
		if(!add_constraintex( lp, 1, row, colno, GE, 0))
			ret = 3;
	}
	free(colno);
	free(row);
	//cout << ret << endl;
	
	//	in - out <= 0
	for( int i = 0 ; i < LP_vertices_group.size() ; i++ )
	{
		colno = (int *) malloc(Ncol * sizeof(*colno));
		row = (REAL *) malloc(Ncol * sizeof(*row));
		counter = 0;
		vector<pair<int,int>> out_vec = LP_out_edges[LP_vertices_group[i]];
		for( int j = 0 ; j < out_vec.size() ; j++ )
		{
			tuple<int,int,int> edge( LP_vertices_group[i].first, LP_vertices_group[i].second, out_vec[j].second );
			int edge_i = LP_edge_map[edge];
			colno[counter] = edge_i;
			row[counter++] = 1;
		}
		
		vector<pair<int,int>> in_vec = LP_in_edges[LP_vertices_group[i]];
		for( int j = 0 ; j < in_vec.size() ; j++ )
		{
			tuple<int,int,int> edge( LP_vertices_group[i].first, in_vec[j].second, LP_vertices_group[i].second );
			int edge_i = LP_edge_map[edge];
			colno[counter] = edge_i;
			row[counter++] = -1;
		}
		if(!add_constraintex( lp, counter, row, colno, LE, 0))
			ret = 4;
		free(colno);
		free(row);
	}
	//cout << ret << endl;

	// vsource - vsink <= 0
	colno = (int *) malloc(Ncol * sizeof(*colno));
	row = (REAL *) malloc(Ncol * sizeof(*row));
	counter = 0;
	vector<pair<int,int>> out_vec = LP_out_edges[make_pair(-1,p_po->vsource)];
	for( int j = 0 ; j < out_vec.size() ; j++ )
	{
		tuple<int,int,int> edge( out_vec[j].first, p_po->vsource, out_vec[j].second );
		int edge_i = LP_edge_map[edge];
		colno[counter] = edge_i;
		row[counter++] = 1;
	}
	
	vector<pair<int,int>> in_vec = LP_in_edges[make_pair(-1,vsink)];
	for( int j = 0 ; j < in_vec.size() ; j++ )
	{
		tuple<int,int,int> edge( in_vec[j].first, in_vec[j].second, vsink );
		int edge_i = LP_edge_map[edge];
		colno[counter] = edge_i;
		row[counter++] = -1;
	}
	if(!add_constraintex( lp, counter, row, colno, LE, 0))
		ret = 5;
	free(colno);
	free(row);
	//cout << ret << endl;

	// f < Nl
	for( int i = 0 ; i < lp_edges_per_ts_per_group.size() ; i++ )
	{
		vector<vector<tuple<int,int,int>>> edge_one_timestep = lp_edges_per_ts_per_group[i];
		for( int j = 0 ; j < edge_one_timestep.size() ; j++ )
		{
			colno = (int *) malloc(Ncol * sizeof(*colno));
			row = (REAL *) malloc(Ncol * sizeof(*row));
			counter = 0;
			vector<tuple<int,int,int>> edge_one_group = edge_one_timestep[j];
			for( int k = 0 ; k < edge_one_group.size() ; k++ )
			{
				tuple<int,int,int> edge = edge_one_group[k];
				int edge_i = LP_edge_map[edge];
				colno[counter] = edge_i;
				row[counter++] = 1;
			}
			if(!add_constraintex( lp, counter, row, colno, LE, max_per_group))
				ret = 6;
			free(colno);
			free(row);
		}
	}
	//cout << ret << endl;

	set_add_rowmode(lp, FALSE);

	set_maxim( lp );
	set_verbose( lp, NEUTRAL );

	ret = solve(lp);

	row = (REAL *) malloc(Ncol * sizeof(*row));
	get_variables(lp, row);

	map<pair<int,int>, int> result_map;
	vector<pair<int,int>> starting_points;

	for( int j = 0 ; j < Ncol ; j++ )
	{
		if( row[j] > 0 )
		{
			tuple<int,int,int> edge = LP_edge_map_reverse[j+1];
			if( edge.get<1>() != p_po->vsource )
			{
				pair<int,int> group_begin( edge.get<0>(), edge.get<1>() );
				result_map[group_begin] = edge.get<2>();
			}
			else
			{
				pair<int,int> group_begin( edge.get<0>(), edge.get<2>() );
				starting_points.push_back( group_begin );
			}
		}
	}

	for( int i = 0 ; i < starting_points.size() ; i++ )
	{
		vector<int> path;
		path.push_back(0);
		pair<int,int> group_begin = starting_points[i];
		while( group_begin.second != vsink )
		{
			path.push_back(group_begin.second);
			int end = result_map[group_begin];
			group_begin = make_pair( group_begin.first, end );
		}
		path.push_back(vsink);
		paths_shitrit.push_back(path);
	}

	//for(int j = 0; j < Ncol; j++)
 	//    printf("%s: %f\n", get_col_name(lp, j + 1), row[j]);

	/*for( int j = 0 ; j < Ncol ; j++ )
	{
		if( row[j] > 0 )
		{
			tuple<int,int,int> edge = LP_edge_map_reverse[j+1];
			std::cout << edge.get<0>() << " : " << edge.get<1>() << " : " << edge.get<2>() << endl;
		}
	}

	for( int i = 0 ; i < paths.size() ; i++ )
	{
		vector<int> path = paths[i];
		for( int j = 0 ; j < path.size() ; j++ )
		{
			cout << path[j] << ", ";
		}
		cout << endl;
	}*/

	/*
	vector<pair<int,int>> edges;
	vector<int> sub_vertices; // TODO: check for duplicates
	map<int,vector<int>> ins, outs;
	map<pair<int,int>, int> edge_map;
	
	int edge_counter = 0;
	int num_of_vars = 0;
	for( int i = 0 ; i < paths.size() ; i++ )
	{
		vector<int> path = paths[i];
		//num_of_vars = num_of_vars + path.size() - 1;
		
		for( int j = 0 ; j < path.size()-1 ; j++ )
		{
			num_of_vars++;
			if( path[j] != p_po->vsource )
			{
				sub_vertices.push_back( path[j] );
			}
			pair<int,int> edge( path[j], path[j+1] );
			edge_map[edge] = edge_counter;
			edges.push_back( edge );
			edge_counter++;
			put( path[j], path[j+1], outs);
			put( path[j+1], path[j], ins);
		}
	}

	lprec *lp;
 	int Ncol, *colno = NULL, j, ret = 0;
 	REAL *row = NULL;

	Ncol = num_of_vars;
	
	lp = make_lp(0, Ncol);

	// objective function
	int counter = 0;
	colno = (int *) malloc(Ncol * sizeof(*colno));
	row = (REAL *) malloc(Ncol * sizeof(*row));
	for( int i = 0 ; i < sub_vertices.size() ; i++ )
	{
		vector<int> out_vec = outs[sub_vertices[i]];
		for( int j = 0 ; j < out_vec.size() ; j++ )
		{
			pair<int,int> edge = make_pair( sub_vertices[i], out_vec[j] );
			int edge_i = edge_map[edge];
			colno[counter] = edge_i;
			row[counter++] = -vertices[sub_vertices[i]-1];
		}
	}
	set_obj_fnex(lp, counter, row, colno);

	free(colno);
	free(row);

	set_add_rowmode(lp, TRUE);

	// f >= 0 && f <= 0
	colno = (int *) malloc(Ncol * sizeof(*colno));
    row = (REAL *) malloc(Ncol * sizeof(*row));
	for( int i = 1 ; i <= edge_counter ; i++ )
	{
		colno[0] = i;
		row[0] = 1;
		add_constraintex( lp, 1, row, colno, GE, 0);
		// TODO: This is wrong, the sum should be <= 1
		add_constraintex( lp, 1, row, colno, LE, 1);
		colno[0] = 0;
		row[0] = 0;
	}
	free(colno);
	free(row);
	
	//	in - out <= 0
	for( int i = 0 ; i < sub_vertices.size() ; i++ )
	{
		colno = (int *) malloc(Ncol * sizeof(*colno));
		row = (REAL *) malloc(Ncol * sizeof(*row));
		counter = 0;
		vector<int> out_vec = outs[sub_vertices[i]];
		for( int j = 0 ; j < out_vec.size() ; j++ )
		{
			pair<int,int> edge = make_pair( sub_vertices[i], out_vec[j] );
			int edge_i = edge_map[edge];
			colno[counter] = edge_i;
			row[counter++] = 1;
		}
		
		vector<int> in_vec = ins[sub_vertices[i]];
		for( int j = 0 ; j < in_vec.size() ; j++ )
		{
			pair<int,int> edge = make_pair( in_vec[j], sub_vertices[i] );
			int edge_i = edge_map[edge];
			colno[counter] = edge_i;
			row[counter++] = -1;
		}
		add_constraintex( lp, counter, row, colno, LE, 0);
		free(colno);
		free(row);
	}

	// vsource - vsink <= 0
	colno = (int *) malloc(Ncol * sizeof(*colno));
	row = (REAL *) malloc(Ncol * sizeof(*row));
	counter = 0;
	vector<int> out_vec = outs[p_po->vsource];
	for( int j = 0 ; j < out_vec.size() ; j++ )
	{
		pair<int,int> edge = make_pair( p_po->vsource, out_vec[j] );
		int edge_i = edge_map[edge];
		colno[counter] = edge_i;
		row[counter++] = 1;
	}
	
	vector<int> in_vec = ins[vsink];
	for( int j = 0 ; j < in_vec.size() ; j++ )
	{
		pair<int,int> edge = make_pair( in_vec[j], vsink );
		int edge_i = edge_map[edge];
		colno[counter] = edge_i;
		row[counter++] = -1;
	}
	add_constraintex( lp, counter, row, colno, LE, 0);
	
	set_add_rowmode(lp, FALSE);

	set_maxim(lp);
	ret = solve(lp);

	get_variables(lp, row);
	for(j = 0; j < Ncol; j++)
 	    printf("%s: %f\n", get_col_name(lp, j + 1), row[j]);
	*/

	if(lp != NULL) {
 	  /* clean up such that all used memory by lpsolve is freed */
 	  delete_lp(lp);
 	}
}

Batch::Batch( PO & po, int begin_frame_counter, vector<location> sources_in )
{
	p_po = &po;
	
	setBatchLimits( po, begin_frame_counter, batch_begin_frame, batch_num_of_frames );
	
	sources = sources_in;
	
	offset = log((1-po.pass_prob) / po.pass_prob);

	readFiles();

	vsink = vertices.size() + 1;

	graph_t g;
	createGraph( g );
	po.first_graph = false;
	g_original = g;

	getSource();

	vector<int> path;
	getShortestPath( g, path );
	paths.push_back(path);

	cout << paths.size() << " path found." << endl;

	double costCurrent = pathsCost();
	double costPrev = 0.0;

	vector_property_map<double> distances;
	map<pair<int, int>, double> weights;

	copy_weight_and_distance_map( g , weights , distance_map , distances );

	for( int i = 1 ; i <= p_po->max_detections ; i++ ) 
	{
		if( i != 1 ) {
			if ( sources.size() == 0 ) 
			{
				costCurrent = pathsCost();
				if( costCurrent >= costPrev ) 
				{
					paths = prevPaths;
					break;
				}
			}
			cout << paths.size() << " paths found." << endl; 
		}
		costPrev = costCurrent;
		prevPaths = paths;
		
		graph_t	g = g_original;
		map<int,int> vertexPairs;

		extendGraph( g, vertexPairs );
		transformEdgeCost( g, weights, distances, vertexPairs );

		getSource();
		vector<int> path;
		getShortestPath( g, path );

		interlace_and_augment( path );

		weights.clear();
		copy_weight_and_distance_map( g , weights , distance_map , distances );
	}
}